
// import haxe.ds.Array;
// import oimo.common.Setting;
// import oimo.common.Vec3;
// import oimo.m.M;

import { EpaVertex } from "./EpaVertex";
import { EpaTriangle } from "./EpaTriangle";
import { Vec3 } from "../../../../common/Vec3";
import { Setting } from "../../../../common/Setting";
import { M } from "../../../../common/M";
import { GjkEpaLog } from "./GjkEpaLog";
import { EpaPolyhedronState } from "./EpaPolyhedronState";

/**
 * numberernal class.
 */
export class EpaPolyhedron {
	_vertices: Array<EpaVertex>;
	_numVertices: number;

	_triangleList: EpaTriangle;
	_triangleListLast: EpaTriangle;
	_numTriangles: number;

	_trianglePool: EpaTriangle;
	_vertexPool: EpaVertex;

	_center: Vec3;
	_status: number;

	constructor() {
		this._vertices = new Array<EpaVertex>(Setting.maxEPAVertices);
		this._center = new Vec3();
		this._numVertices = 0;
		this._triangleList = null;
		this._triangleListLast = null;
		this._numTriangles = 0;
		this._trianglePool = null;
		this._vertexPool = null;
	}

	// --- --- private
	private pickTriangle(): EpaTriangle {
		if(this._trianglePool) {
			return new EpaTriangle();
		} else {
			let result = this._trianglePool;
			this._trianglePool = this._trianglePool._next;
			result._next = null;
			return result;
		}
	}


	private poolTriangle(t: EpaTriangle): void {
		t.removeReferences();
		t._next = this._trianglePool;
		this._trianglePool = t;
	}


	private setAdjacentTriangle(t1: EpaTriangle, t2: EpaTriangle): void {
		if (!t1.setAdjacentTriangle(t2)) {
			this._status = EpaPolyhedronState.INVALID_TRIANGLE;
		}
	}

	private initTriangle(t: EpaTriangle, vertex1: EpaVertex, vertex2: EpaVertex, vertex3: EpaVertex, center: Vec3, autoCheck: boolean = false): void {
		if (!t.init(vertex1, vertex2, vertex3, center, autoCheck)) {
			this._status = EpaPolyhedronState.INVALID_TRIANGLE;
		}
	}

	public validate(): boolean {
		let t = this._triangleList;
		while (t != null) {
			for (let i = 0; i < 3; ++i) {
				t._vertices[i]._tmpEdgeLoopOuterTriangle = null;
				t._vertices[i]._tmpEdgeLoopNext = null;
				if (t._adjacentPairIndex[i] == -1) {
					this._status = EpaPolyhedronState.NO_ADJACENT_PAIR_INDEX;
					return false;
					//throw M.error("!?"));
				}
				if (t._adjacentTriangles[i] == null) {
					this._status = EpaPolyhedronState.NO_ADJACENT_TRIANGLE;
					return false;
					//throw M.error("!?"));
				}
			}
			t = t._next;
		}
		return true;
	}

	public findEdgeLoop(id: number, base: EpaTriangle, from: Vec3): void {
		if (base._tmpDfsId == id) return;
		base._tmpDfsId = id;
		GjkEpaLog.log("DFS: " + base.id);
		if (!base.checkVisible(id, from)) {
			this._status = EpaPolyhedronState.TRIANGLE_INVISIBLE;
			GjkEpaLog.log("tri " + base.id + " is invisible!");
			return;
		}

		// find edges of the hole
		for (let i = 0; i < 3; ++i) {
			var t: EpaTriangle = base._adjacentTriangles[i];
			if (t == null) continue;
			if (t.checkVisible(id, from)) {
				GjkEpaLog.log("tri " + t.id + " is visible.");
				this.findEdgeLoop(id, t, from);
			} else {
				// triangle `base` can be seen from `from`, but triangle `t` cannot.
				GjkEpaLog.log("tri " + t.id + " is invisible.");
				GjkEpaLog.log("added edge: " + base.id + " " + t.id);
				var i2: number = base._nextIndex[i];
				var v1: EpaVertex = base._vertices[i];
				var v2: EpaVertex = base._vertices[i2];
				v1._tmpEdgeLoopNext = v2;
				v1._tmpEdgeLoopOuterTriangle = t;
			}
		}

		// expand the hole
		base.removeAdjacentTriangles();
		this.removeTriangle(base);
	}

	private addTriangle(t: EpaTriangle): void {
		this._numTriangles++;
		// GjkEpaLog.log("triangle added " + this._numTriangles + ", id: " + t.id);
		if (this._triangleList == null) {
			this._triangleList = t;
			this._triangleListLast = t;
		} else {
			this._triangleListLast._next = t;
			t._prev = this._triangleListLast;
			this._triangleListLast = t;
		}
	}

	private removeTriangle(t: EpaTriangle): void {
		this._numTriangles--;
		// GjkEpaLog.log("triangle removed " + this._numTriangles + ", id: " + t.id);
		let prev = t._prev;
		let next = t._next;
		if (prev != null) {
			prev._next = next;
		}
		if (next != null) {
			next._prev = prev;
		}
		if (t == this._triangleList) {
			this._triangleList = next;
		}
		if (t == this._triangleListLast) {
			this._triangleListLast = prev;
		}
		t._next = null;
		t._prev = null;

		// M.list_remove(this._triangleList, this._triangleListLast, _prev, _next, t);
		this.poolTriangle(t);
	}

	// --- internal ---
	public _pickVertex(): EpaVertex {
		if (this._vertexPool == null) {
			return new EpaVertex();
		} else {
			var result = this._vertexPool;
			this._vertexPool = this._vertexPool._next;
			result._next = null;
			return result;
		}
	}


	public _poolVertex(v: EpaVertex): void {
		v.removeReferences();
		v._next = this._vertexPool;
		this._vertexPool = v;
	}

	public _clear(): void {
		while (this._numTriangles > 0) {
			this.removeTriangle(this._triangleList);
		}
		while (this._numVertices > 0) {
			this._poolVertex(this._vertices[--this._numVertices]);
		}
	}

	public _init(v1: EpaVertex, v2: EpaVertex, v3: EpaVertex, v4: EpaVertex): boolean {
		this._status = EpaPolyhedronState.OK;
		this._numVertices = 4;
		this._vertices[0] = v1;
		this._vertices[1] = v2;
		this._vertices[2] = v3;
		this._vertices[3] = v4;
		this._center.copyFrom(v1.v).addEq(v2.v).addEq(v3.v).addEq(v4.v).scaleEq(0.25);
		var t1: EpaTriangle;
		var t2: EpaTriangle;
		var t3: EpaTriangle;
		var t4: EpaTriangle;
		t1 = this.pickTriangle();
		t2 = this.pickTriangle();
		t3 = this.pickTriangle();
		t4 = this.pickTriangle();
		this.initTriangle(t1, v1, v2, v3, this._center, true);
		this.initTriangle(t2, v1, v2, v4, this._center, true);
		this.initTriangle(t3, v1, v3, v4, this._center, true);
		this.initTriangle(t4, v2, v3, v4, this._center, true);
		this.setAdjacentTriangle(t1, t2);
		this.setAdjacentTriangle(t1, t3);
		this.setAdjacentTriangle(t1, t4);
		this.setAdjacentTriangle(t2, t3);
		this.setAdjacentTriangle(t2, t4);
		this.setAdjacentTriangle(t3, t4);
		this.addTriangle(t1);
		this.addTriangle(t2);
		this.addTriangle(t3);
		this.addTriangle(t4);
		return this._status == EpaPolyhedronState.OK;
	}


	public _getBestTriangle(): EpaTriangle {
		var f: EpaTriangle = this._triangleList;
		var mind: number = 1.0 / 0;
		var minf: EpaTriangle = null;
		while (f != null) {
			if (f._distanceSq < mind) {
				mind = f._distanceSq;
				minf = f;
			}
			f = f._next;
		}
		return minf;
	}

	public _addVertex(vertex: EpaVertex, base: EpaTriangle): boolean {
		this._vertices[this._numVertices++] = vertex;
		GjkEpaLog.log("vertex added " + this._numVertices + " " + vertex.v);
		GjkEpaLog.log("begin polyhedron modifying...");

		var v1: EpaVertex = base._vertices[0];

		GjkEpaLog.log("trying to find a edge loop... v=" + vertex.v);
		// make a hole on the polyhedron finding its edge loop
		this.findEdgeLoop(this._numVertices, base, vertex.v);
		if (this._status != EpaPolyhedronState.OK) return false;

		// ... and "patch" the hole
		var v: EpaVertex = v1;
		var firstV: EpaVertex = v1;
		var prevT: EpaTriangle = null;
		var firstT: EpaTriangle = null;
		do {
			if (v._tmpEdgeLoopNext == null) {
				GjkEpaLog.log("edge loop is broken:");
				this._status = EpaPolyhedronState.EDGE_LOOP_BROKEN;
				return false;
			}
			if (v._tmpEdgeLoopOuterTriangle == null) {
				this._status = EpaPolyhedronState.NO_OUTER_TRIANGLE;
				return false;
			}

			var t: EpaTriangle = this.pickTriangle();
			if (firstT == null) firstT = t;
			GjkEpaLog.log("patching...");

			this.initTriangle(t, v, v._tmpEdgeLoopNext, vertex, this._center);
			if (this._status != EpaPolyhedronState.OK) return false;
			this.addTriangle(t);

			this.setAdjacentTriangle(t, v._tmpEdgeLoopOuterTriangle);
			if (prevT != null) this.setAdjacentTriangle(t, prevT);

			prevT = t;

			v = v._tmpEdgeLoopNext;
		} while (v != firstV);
		this.setAdjacentTriangle(prevT, firstT);

		return this._status == EpaPolyhedronState.OK && this.validate();
	}

}
